//前轮万向轮，后轮驱动加转向，以成功调试

#include <BluetoothSerial.h>
#include <Arduino.h>
#include <ESP32Servo.h>
BluetoothSerial ESP32_BT;

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#define PWMA 5       //TB6612的相关引脚
#define AIN2 19
#define AIN1 18
//#define STBY 27     //内部拉高了
#define BIN1 17
#define BIN2 16
#define PWMB 4
#define MAX_SPEED 200     //0-255
int lpad_x=128,lpad_y=128;
char BluetoothData;         //蓝牙数据
void tb6612_init(){
  pinMode(AIN2,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  
  pinMode(PWMA, OUTPUT);
  pinMode(PWMA, OUTPUT);

}
void setup(){
   tb6612_init();
   ESP32_BT.begin("BTcar2");
   Serial.begin(115200);       // 启动硬件串口（用于输出调试信息和接收控制指令）
   delay(100);
    
}
void loop(){
   if(ESP32_BT.available()){
      BluetoothData=ESP32_BT.read();
      switch(BluetoothData){
        case'1':forward();break;
        case'2':right();break;
        case'3':down();break;
        case'4':left();break;
        case'5':up_right();break;
        case'6':down_right();break;
        case'7':down_left();break;
        case'8':up_left();break;
        case'o':stopcar();break;
        case'X':              //左摇杆
            lpad_x=ESP32_BT.parseInt();
            while (BluetoothData!='*'){
                if(ESP32_BT.available()){
                    BluetoothData=ESP32_BT.read();   //Get next character from bluetooth
                    if(BluetoothData=='Y')   lpad_y=ESP32_BT.parseInt();
                }
             }   
            judge();
            break;    
      }
   }
}
void judge(){
  int X = lpad_x;
  int Y = lpad_y;
  int flag;
  if(X>50&&X<200&&Y>50&&Y<200){
    stopcar();
  }else if(X>50&&X<200&&Y<50){
    forward();
  }else if(X>50&&X<200&&Y>200){
    down();
  }else if(X<50&&Y<50){
    up_left();
  }else if(X>200&&Y<50){
   up_right();
  }else if(X>200&&Y>200){
    down_right();
  }else if(X<50&&Y>200){
    down_left();
  }else if(Y>50&&Y<200&&X<50){
    left();
  }else if(Y>50&&Y<200&&X>200){
    right();
  }
  
}
void forward(){
  digitalWrite (AIN1,0);
  digitalWrite (AIN2,1);
  digitalWrite (BIN1,0);
  digitalWrite (BIN2,1);
  analogWrite(PWMA, 180);
  analogWrite(PWMB, 180);
}
void down(){
  digitalWrite (AIN1,1);
  digitalWrite (AIN2,0);
  digitalWrite (BIN1,1);
  digitalWrite (BIN2,0);
  analogWrite(PWMA, 180);
  analogWrite(PWMB, 180);
}
void left(){
  digitalWrite (AIN1,0);
  digitalWrite (AIN2,0);
  digitalWrite (BIN1,0);
  digitalWrite (BIN2,1);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 180);
}

void right(){
  digitalWrite (AIN1,0);
  digitalWrite (AIN2,1);
  digitalWrite (BIN1,0);
  digitalWrite (BIN2,0);
  analogWrite(PWMA, 180);
  analogWrite(PWMB, 0);
}
void up_left(){
  digitalWrite (AIN1,0);
  digitalWrite (AIN2,1);
  digitalWrite (BIN1,0);
  digitalWrite (BIN2,1);
  analogWrite(PWMA, 60);
  analogWrite(PWMB, 150);
}
void up_right(){
  digitalWrite (AIN1,0);
  digitalWrite (AIN2,1);
  digitalWrite (BIN1,0);
  digitalWrite (BIN2,1);
  analogWrite(PWMA, 150);
  analogWrite(PWMB, 60);
}
void down_right(){
  digitalWrite (AIN1,1);
  digitalWrite (AIN2,0);
  digitalWrite (BIN1,1);
  digitalWrite (BIN2,0);
  analogWrite(PWMA, 150);
  analogWrite(PWMB, 60);
}
void down_left(){
  digitalWrite (AIN1,1);
  digitalWrite (AIN2,0);
  digitalWrite (BIN1,1);
  digitalWrite (BIN2,0);
  analogWrite(PWMA, 60);
  analogWrite(PWMB, 150);
}
void stopcar(){
  digitalWrite (AIN1,0);
  digitalWrite (AIN2,0);
  digitalWrite (BIN1,0);
  digitalWrite (BIN2,0);
  analogWrite(PWMA, 30);
  analogWrite(PWMB, 30);
}

